/**
 * @file depth_cluster.cpp
 * @author tangjf (ryontang@163.com)
 * @brief 
 * @version 0.1
 * @date 2020-12-07
 * 
 * @copyright Copyright (c) 2020
 * 
 */

//#include "depth_cluster.h"
#include "n_lidar_obj/depth_cluster/depth_cluster.h"
//#include "image_labelers/linear_image_labeler.h"

using cv::DataType;
using cv::Mat;
using std::to_string;

const cv::Point ANCHOR_CENTER = cv::Point(-1, -1);
const int SAME_OUTPUT_TYPE = -1;

void Depth_cluster::Init()
{
  _ground_cloud = make_shared<Cloud>(); // _ground_cloud指向一个动态分配的空Cloud
  _except_ground_cloud = make_shared<Cloud>();

  this->SpanParams(-180_deg, 180_deg, 870);
  size_t cols = _num_beams;
  for (size_t c = 0; c < cols; c++)
  {
    _col_angles.push_back(_start_angle + _step * c);
  }
  std::cout << "Initionalization was finished!!" << std::endl;
}

void Depth_cluster::SpanParams(const Radians &start_angle,
                               const Radians &end_angle, int num_beams)
{
  _start_angle = start_angle;
  _end_angle = end_angle;
  _num_beams = num_beams;

  // 角度除以固定区域，就是每一个区域的角度分辨率(弧度值)在变化
  _step = (_end_angle - _start_angle) / _num_beams;
  _span = Radians::Abs(end_angle - start_angle);
}

void Depth_cluster::CloudProjection(
    const msg_lidar_prep::msg_lidar_prep::ConstPtr &points)
{
  // 初始化每一次，清零的作用
  _depth_image =
      cv::Mat::zeros(32, _num_beams, cv::DataType<float>::type); // 870+1
  _repaired_depth_image =
      cv::Mat::zeros(_depth_image.size(), cv::DataType<float>::type);
  _angle_image =
      cv::Mat::zeros(_depth_image.size(), cv::DataType<float>::type);
  _smoothed_angle_image =
      cv::Mat::zeros(_depth_image.size(), cv::DataType<float>::type);
  _no_ground_image =
      cv::Mat::zeros(_depth_image.size(), cv::DataType<float>::type);

  _xy_image = cv::Mat::zeros(_depth_image.size(), cv::DataType<float>::type);
  _z_image = cv::Mat::zeros(_depth_image.size(), cv::DataType<float>::type);
  _label_image =
      cv::Mat::zeros(_depth_image.size(), cv::DataType<uint16_t>::type);
  _no_ground_label_image =
      cv::Mat::zeros(_depth_image.size(), cv::DataType<uint16_t>::type);
  _angle_arith_image =
      cv::Mat::zeros(_depth_image.size(), cv::DataType<float>::type);
  _angle_vert_image =
      cv::Mat::zeros(_depth_image.size(), cv::DataType<float>::type);

  // new loop init some variable
  _count_ground = 0;
  // Cloud::Ptr cloud(new Cloud);
  Cloud::Ptr cloud;
  cloud = make_shared<Cloud>();

  PointMatrix data = PointMatrix(_num_beams, PointColumn(32));

  cout << "\n"
       << endl;
  cout << "*************************************************************"
       << endl;
  cout << "point size: " << points->lidar_prep.size() << endl;

  for (size_t index = 0; index < points->lidar_prep.size(); ++index)
  {
    const auto point = points->lidar_prep[index]; // todo:传引用

    RichPoint point_tem;
    // 2D距离太近
    float dist_to_sensor =
        sqrt(point.x * point.x + point.y * point.y + point.z * point.z);

    // 距离小于0.01跳过
    point_tem.x() = point.x;
    point_tem.y() = point.y;
    point_tem.z() = point.z;
    point_tem.intensity() = point.intensity;
    float angle_cols = point.angle * 0.01;

    cloud->push_back(point_tem);

    auto temp_angle_cols = Radians::FromRadians(atan2(point.y, point.x));
    // 确定了列数
    size_t bin_cols = this->ColFromAngle(temp_angle_cols);

    // 调试改参1
    // 位于深度图哪一行，与我们采取的算法保持一致，0线束的点放在Mat的第31行
    size_t bin_rows = 31 - point.ring;
    // size_t bin_rows = point.ring;

    // 先列再行
    data[bin_cols][bin_rows].points().push_back(index);

    // todo:传引用, 深度图某行某列的距离,后续要用赋值
    float &current_written_depth = _depth_image.at<float>(bin_rows, bin_cols);
    float &current_written_z = _z_image.at<float>(bin_rows, bin_cols);
    float &current_written_xy = _xy_image.at<float>(bin_rows, bin_cols);
    float &current_arith_angle =
        _angle_arith_image.at<float>(bin_rows, bin_cols);
    float &current_vert_angle = _angle_vert_image.at<float>(bin_rows, bin_cols);

    // 没有被push值，该格子初始化
    if (current_written_xy < 0.001f)
    {
      current_written_z = point.z;
      current_written_xy = sqrt(point.x * point.x + point.y * point.y);
      current_written_depth = dist_to_sensor;
      current_arith_angle = angle_cols;
      current_vert_angle =
          atan(current_written_z / current_written_xy) * 180.0 / M_PI;
      continue;
    }

    // 会有重复的点
    if (current_written_depth < dist_to_sensor)
    {
      // write this point to the image only if it is closer仅当更近时
      // 如果同一分辨率有两个点，取更近的一个点 = current_written_depth *
      // _vres[31-bin_rows];
      current_written_depth = dist_to_sensor;
      current_written_xy = sqrt(point.x * point.x + point.y * point.y);
      current_written_z = point.z;
      current_arith_angle = angle_cols;
      // 反正是0~90 用atan atan2都可以
      // value = asinx, value为-pi/2, pi/2
      current_vert_angle =
          atan(current_written_z / current_written_xy) * 180.0 / M_PI;
    }
  }
  // this->RvizVisual(cloud, _pub_cloud);
  _repaired_depth_image = this->RepairDepth(_depth_image, 5, 1.0f);
  _angle_image = this->CreateAngleImage(_repaired_depth_image);

  // 可视化sensor_msgs::Image
  // auto temp = this->DepthImage_to_ImageMsg(_depth_image, 60);
  // _image_pub.publish(temp);

  _smoothed_angle_image =
      this->ApplySavitskyGolaySmoothing(_angle_image, _window_size);

  // _ground_cloud已在里面被赋值
  this->MYZeroOutGroundBFS(_repaired_depth_image, _smoothed_angle_image, _no_ground_image,
                           _label_image, _ground_remove_angle, _window_size,
                           data, cloud);

  // auto temp = this->DepthImageAndAngleImage_to_ImageMsg(_depth_image, _repaired_depth_image, _angle_image, _smoothed_angle_image, _no_ground_image);
  // _image_pub.publish(temp);

  // voxel聚类
  lidar::obj::Curvevoxel_cluster voxel_cluster;
  std::vector<lidar::obj::PointAPR> papr;

  // 点云输入
  /// voxel_cluster.calculateAPR();
  for (int i = 0; i < _except_ground_cloud->size(); i++)
  {
    lidar::obj::PointAPR par;
    // top view: polar_angle水平角
    par.polar_angle = voxel_cluster.Polar_angle_cal(_except_ground_cloud->at(i).x(), _except_ground_cloud->at(i).y());
    par.range = sqrt(_except_ground_cloud->at(i).x() * _except_ground_cloud->at(i).x() + _except_ground_cloud->at(i).y() * _except_ground_cloud->at(i).y());
    // azimuth垂直角
    par.azimuth = (float)atan2(_except_ground_cloud->at(i).z(), par.range);

    if (par.azimuth < voxel_cluster.minazimuth)
    {
      voxel_cluster.minazimuth = par.azimuth;
    }
    else if (par.azimuth > voxel_cluster.maxazimuth)
    {
      voxel_cluster.maxazimuth = par.azimuth;
    }
    if (par.range < voxel_cluster.minrange)
    {
      voxel_cluster.minrange = par.range;
    }
    else if (par.range > voxel_cluster.maxrange)
    {
      voxel_cluster.maxrange = par.range;
    }
    papr.push_back(par);
  }
  cout << "_except_ground_cloud size:" << _except_ground_cloud->size() << endl;
  cout << "papr size:" << papr.size() << endl;

  voxel_cluster.length = std::round((voxel_cluster.maxrange - voxel_cluster.minrange) / voxel_cluster.deltaR);
  // 立体扇形voxel的宽度个数确定
  //voxel_cluster.width = 301;
  voxel_cluster.width = std::round(360.0 / voxel_cluster.deltaP);
  //voxel_cluster.height = std::round((voxel_cluster.maxazimuth - voxel_cluster.minazimuth) / voxel_cluster.deltaA);
  // revised in 9-23
  // maxazimuth-minazimuth要转换成度数
  voxel_cluster.height = std::round(((voxel_cluster.maxazimuth - voxel_cluster.minazimuth) * 180 / M_PI) / voxel_cluster.deltaA);
  /// voxel_cluster.calculateAPR();

  std::unordered_map<int, lidar::obj::Voxel> hvoxel;

  voxel_cluster.build_hash_table(papr, hvoxel);

  // cluster_index与papr大小一致，cluster_index存着聚类的id
  std::vector<int> cluster_index = voxel_cluster.CVC(hvoxel, papr);
  cout << "cluster_index size:" << cluster_index.size() << endl;

  // 访问公共资源_obj_cluster先上锁
  std::unique_lock<std::mutex> locker(_mu);

  // 聚类id已经出来，开始筛选
  for (size_t i = 0; i < cluster_index.size(); i++)
  {
    const auto point = _except_ground_cloud->points()[i];
    _obj_cluster[cluster_index.at(i)].push_back(point);
  }
  //this->RvizVisual(_except_ground_cloud, _pub_depth_cluster);
  _except_ground_cloud->clear(); // 智能指针自动消除

  std::vector<uint16_t> labels_to_erase;

  for (const auto &kv : _obj_cluster)
  {
    const auto &cluster = kv.second;
    if (cluster.size() < _min_cluster_size ||
        cluster.size() > _max_cluster_size)
    {
      labels_to_erase.push_back(kv.first);
    }

    // 遍历，马路两边的就不关注了
    float sum_x = 0;
    float sum_y = 0;
    float x_min = INT_MAX;
    float x_max = INT_MIN;
    float y_min = INT_MAX;
    float y_max = INT_MIN;

    for (int i = 0; i < cluster.size(); ++i)
    {
      sum_x += cluster[i].x();
      sum_y += cluster[i].y();
      if (x_min > cluster[i].x())
        x_min = cluster[i].x();
      if (y_min > cluster[i].y())
        y_min = cluster[i].y();
      if (x_max < cluster[i].x())
        x_max = cluster[i].x();
      if (y_max < cluster[i].y())
        y_max = cluster[i].y();
    }

    // 利用边框的坐标信息做一些筛除
    float area = (y_max - y_min) * (x_max - x_min);
    float locate_x = sum_x / cluster.size();
    if ((area - 50.0) > 0.01 || fabs(locate_x) - 10.0 > 0.01 || (y_max - y_min) - 10.0 > 0.01 || (x_max - x_min) - 10.0 > 0.01)
      labels_to_erase.push_back(kv.first);
  }
  for (auto label : labels_to_erase)
  {
    _obj_cluster.erase(label); // 可以把这些cluster用PCL可视化了
  }

  cout << "_obj_cluster size:" << _obj_cluster.size() << endl;
  // 提醒发布flag
  _pub_flag = 1;

  // 解锁
  locker.unlock();

  // 通知发布线程
  //_cond.notify_one(); // 激活等待这个条件的线程|告诉消费者此刻你可以消费了

  _ground_cloud->clear();

  // 赋值给基类cluster的成员
  //_obj_cluster.clear();
}

size_t Depth_cluster::ColFromAngle(const Radians &angle)
{
  // 输入角度的弧度值 _col_angles: 原始的水平角
  return FindClosest(_col_angles, angle);
}

size_t Depth_cluster::FindClosest(const std::vector<Radians> &vec,
                                  const Radians &val)
{
  size_t found = 0;

  // 正常，vec.back()没有超过360度
  if (vec.front() < vec.back())
  {
    found = upper_bound(vec.begin(), vec.end(), val) - vec.begin();
  }
  else
  {
    found = vec.rend() - upper_bound(vec.rbegin(), vec.rend(), val);
  }
  if (found == 0)
  {
    return found;
  }
  if (found == vec.size())
  {
    return found - 1;
  }
  auto diff_next = Radians::Abs(vec[found] - val);
  auto diff_prev = Radians::Abs(val - vec[found - 1]);
  return diff_next < diff_prev ? found : found - 1;
}

Mat Depth_cluster::CreateAngleImage(const cv::Mat &depth_image)
{
  // 尽量按照与type对应的数据类型存取数据，
  // 如果你不清楚数据类型对应的type是什么，以float为例，可以用DataType<float>::type来获取
  cv::Mat angle_image =
      cv::Mat::zeros(depth_image.size(), cv::DataType<float>::type);
  auto startTime = std::chrono::steady_clock::now();

  // 每个点的水平角度与垂直角度需要保存
  int temp_count = 0;
  for (int r = 1; r < angle_image.rows; ++r)
  {
    for (int c = 0; c < angle_image.cols; ++c)
    {
      // d_xy之差
      float dx =
          fabs(_xy_image.at<float>(r, c) - _xy_image.at<float>(r - 1, c));

      // d_z之差
      float dz = fabs(_z_image.at<float>(r, c) - _z_image.at<float>(r - 1, c));

      // 调参数2
      // r为1~31 求a角 存度数  返回0～90
      angle_image.at<float>(r, c) = atan2(dz, dx) * 180.0 / M_PI;
      // angle_image.at<float>(r - 1, c) = atan2(dz, dx) * 180.0 / M_PI;
    }
  }

  auto endTime = std::chrono::steady_clock::now();
  auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(
      endTime - startTime);
  std::cout << "CreateAngleImage took " << elapsedTime.count()
            << " milliseconds" << std::endl;
  return angle_image;
}

// 检测地面点 调试改参2： threshold 地面起始滤除角度
int Depth_cluster::MYZeroOutGroundBFS(
    const cv::Mat &depth_image, // image是depth_image
    const cv::Mat &angle_image, cv::Mat &no_ground_image, cv::Mat &label_image,
    const float &start_ground_threshold, int kernel_size, PointMatrix &datalist,
    const Cloud::Ptr cloud_set)
{
  // todo：使用init初始化params
  auto startTime = std::chrono::steady_clock::now();
  // ClustererT
  // Radians start_thresh = 30_deg;

  for (int c = 0; c < angle_image.cols; ++c)
  {
    // start at bottom pixels and do bfs todo：BFS
    // 总共32行从下标31开始
    int r = angle_image.rows - 1;
    // 调参 最低行必须全部检查到
    // while (r > 0 && depth_image.at<float>(r, c) < 0.0001f && r != 31) //
    // image是depth_image todo: r不能为0，若为0则angle_image无元素
    while (r > 0 && depth_image.at<float>(r, c) < 0.0001f)
    {
      r = r - 1;
    }

    auto current_coord = PixelCoord(r, c);
    uint16_t current_label = label_image.at<uint16_t>(r, c);

    // TODO(igor): this is a test. Maybe switch it on, maybe off.
    // if (fabs(angle_image.at<float>(r, c)) > start_ground_threshold ||
    // current_label > 0) // to do 初始行的角度超过了阈值，肯定不是地面，跳过

    if (current_label > 0)
    {
      continue;
    }

    // if (fabs(angle_image.at<float>(r, c)) < start_ground_threshold && r ==
    // 31) // start_ground_threshold只针对最低行
    if (fabs(angle_image.at<float>(r, c)) < start_ground_threshold)
    {
      int label = 1;

      // BFS label angle_image存度数
      this->ground_BFS(current_coord, label, label_image, depth_image,
                       angle_image, _ground_tollerance);
    }
  }

  // 膨胀算法暂时不考虑
  auto label_image_ptr = &label_image;
  if (label_image_ptr->rows != _no_ground_image.rows ||
      label_image_ptr->cols != _no_ground_image.cols)
  {
    fprintf(stderr, "ERROR: label image and res do not correspond.\n");
    // return res;
  }

  // dilated arg
  kernel_size = std::max(kernel_size - 2, 3);
  Mat kernel = GetUniformKernel(kernel_size, CV_8U);
  Mat dilated = Mat::zeros(label_image_ptr->size(), label_image_ptr->type());
  cv::dilate(*label_image_ptr, dilated, kernel); // 膨胀实现dilate

  // Cloud::Ptr ground_cloud(new Cloud)，new了一个空间, 但是我没有delete,
  // 所以改用智能指针
  // Cloud::Ptr ground_cloud;
  // ground_cloud = make_shared<Cloud>();

  // 访问公共资源_ground_cloud先上锁
  std::unique_lock<std::mutex> locker(_mu);

  // Cloud ground_cloud;
  const cv::Mat *labels_dilated_ptr = &dilated;
  for (int row = 1; row < labels_dilated_ptr->rows; ++row)
  {
    for (int col = 0; col < labels_dilated_ptr->cols; ++col)
    {
      // 先col再行
      PointContainer *point_container = &datalist[col][row - 1];
      uint16_t label = labels_dilated_ptr->at<uint16_t>(row, col);

      if (point_container->IsEmpty())
      {
        // this is ok, just continue, nothing interesting here, no points.
        continue;
      }
      // 地面点的标签值为1.0f
      else if (fabs(label - 1) < 0.0001f)
      {
        for (const auto &point_idx : point_container->points())
        {
          auto point = cloud_set->points()[point_idx];
          _ground_cloud->push_back(point);
          _count_ground++;
          // ground_cloud.push_back(point);
        }
      }

      // 非地面点标签值为0.0f
      else
      {
        _no_ground_image.at<float>(row, col) = depth_image.at<float>(row, col);
        for (const auto &point_idx : point_container->points())
        {
          // 传的都是指针效率高
          auto point = cloud_set->points()[point_idx];
          _except_ground_cloud->push_back(point);
        }
      }
    }
  }

  // unlock()
  locker.unlock();

  // std::unique_lock<std::mutex> locker(_mu);
  // 访问地面点的共享内存，因此要保护 _ground_cloud = ground_cloud;
  // locker.unlock();
  // ground_cloud->clear();
  // Rviz可视化
  // this->RvizVisual(ground_cloud, _pub_ground_cluster);
  // ground_cloud->clear();

  auto endTime = std::chrono::steady_clock::now();
  auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(
      endTime - startTime);
  std::cout << "MYZeroOutGroundBFS took " << elapsedTime.count()
            << " milliseconds" << std::endl;
  cout << "ground 打标签的数量" << _count_ground << endl;
  _count_ground = 0;

  return 0;
}

int Depth_cluster::Cluster(const cv::Mat &no_ground_depth_image,
                           cv::Mat &no_ground_label_image,
                           PointMatrix &datalist, Cloud::Ptr cloud_set,
                           const float &angle_tollerance, int min_cluster_size,
                           int max_cluster_size)
{
  auto startTime = std::chrono::steady_clock::now();
  // 初始 label
  // 之前地面点标签为1，现在非地面点从1开始打标签，重新创一个标签image：no_ground_label_image
  int label = 1;

  // 从0行开始，对应sky
  for (int r = 1; r < no_ground_depth_image.rows; ++r)
  {
    for (int c = 0; c < no_ground_depth_image.cols; ++c)
    {
      // if (label_image.at<int>(r, c) == 0) // 非地面点，再次确认
      //{
      auto current_coord = PixelCoord(r, c);
      uint16_t current_label = no_ground_label_image.at<uint16_t>(r, c);

      // depth equal 0 or label equal 1 just continue, for point in (r,c) is
      // non_point or point
      if (no_ground_depth_image.at<float>(r, c) < 0.0001f ||
          current_label > 0.01f)
      {
        continue;
      }

      this->One_Label_Bfs(current_coord, label, no_ground_label_image,
                          no_ground_depth_image, angle_tollerance);
      label = label + 1;
      //}
    }
  }
  auto endTime = std::chrono::steady_clock::now();
  auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(
      endTime - startTime);
  std::cout << "Cluster bfs label took " << elapsedTime.count()
            << " milliseconds" << std::endl;

  cout << "total label/cluster: " << label << endl;

  auto startTime1 = std::chrono::steady_clock::now();
  // 过滤掉点数量较多或较少的类别
  // create 3d clusters from image labels

  const cv::Mat *labels_ptr = &no_ground_label_image;
  // std::unordered_map<uint16_t, Cloud> clusters;
  // Cloud::Ptr ground_cloud(new Cloud); // 地面点 Cloud::Ptr智能指针，自动销毁
  // Cloud* cloud_ptr = &_cloud;

  // 访问公共资源_obj_cluster先上锁
  std::unique_lock<std::mutex> locker(_mu);

  // 修改参数 row - 1 = row
  for (int row = 1; row < labels_ptr->rows; ++row)
  {
    for (int col = 0; col < labels_ptr->cols; ++col)
    {
      // const auto &point_container = _cloud->projection_ptr()->at(row, col);

      uint16_t label = labels_ptr->at<uint16_t>(row, col);
      if (label < 1)
      {
        continue;
      }
      // 修改参数 row - 1 = row
      // 先col再行
      PointContainer *point_container = &datalist[col][row - 1];
      if (point_container->IsEmpty())
      {
        // this is ok, just continue, nothing interesting here, no points.
        continue;
      }

      // 取list的值，每个值为point的序号
      for (const auto &point_idx : point_container->points())
      {
        const auto &point = cloud_set->points()[point_idx];
        _obj_cluster[label].push_back(point);
        // clusters[label].push_back(point);
      }
    }
  }

  auto endTime1 = std::chrono::steady_clock::now();
  auto elapsedTime1 = std::chrono::duration_cast<std::chrono::milliseconds>(
      endTime1 - startTime1);
  std::cout << "Cluster get point took " << elapsedTime1.count()
            << " milliseconds" << std::endl;

  auto startTime2 = std::chrono::steady_clock::now();

  // filter out unfitting clusters
  _cluster_count = 0;
  std::vector<uint16_t> labels_to_erase;
  for (const auto &kv : _obj_cluster)
  {
    const auto &cluster = kv.second;
    _cluster_count++;
    if (cluster.size() < min_cluster_size ||
        cluster.size() > max_cluster_size)
    {
      labels_to_erase.push_back(kv.first);
    }
  }
  for (auto label : labels_to_erase)
  {
    _obj_cluster.erase(label); // 可以把这些cluster用PCL可视化了
    _cluster_count--;          // 最终cluster的类别
  }

  _pub_flag = 1;

  // 解锁
  locker.unlock();

  // 通知发布线程
  _cond.notify_one(); // 激活等待这个条件的线程|告诉消费者此刻你可以消费了

  auto endTime2 = std::chrono::steady_clock::now();
  auto elapsedTime2 = std::chrono::duration_cast<std::chrono::milliseconds>(
      endTime2 - startTime);
  std::cout << "Cluster whole took " << elapsedTime2.count() << " milliseconds"
            << "cluster nums:" << _cluster_count << std::endl;
  _cluster_count = 0;
  // return clusters;
  return 0;
}

void Depth_cluster::Rviz()
{
  Cloud::Ptr ground_cloud(new Cloud); // new一个空间后再深复制
  std::unordered_map<uint16_t, Cloud> clusters;
  // 加线程锁
  std::unique_lock<std::mutex> locker(_mu);
  _cond.wait(locker, [this]() { return 1 == _pub_flag && 1 == _rviz_pub; });
  // 传递数据
  // 深拷贝 地面点of depth_cluster
  for (size_t i = 0; i < _ground_cloud->size(); i++)
  {
    ground_cloud->push_back(_ground_cloud->at(i));
  }

  clusters.swap(_obj_cluster);
  // 数据清空
  // _obj_cluster.clear();  // 暂时不需要，因为每一次循环map都更新
  _ground_cloud->clear(); // 暂时不需要每一次循环cloud::Ptr都更新

  _cluster_count = 0;
  _rviz_pub = 0;
  _pub_flag = 0;
  locker.unlock();

  cout << "rviz clusters size: " << clusters.size() << endl;

  // 1调用聚类显示算法simple
  // 1visualization_msgs::MarkerArray marker_array =
  // this->renderMarker(clusters);
  // 1_pub_marker_array.publish(marker_array);

  // 调用rviz
  //this->RvizVisual(clusters, _pub_depth_cluster);

  auto startTime = std::chrono::steady_clock::now();

  /*
  for (int i = 0; i < clusters.size(); i++)
  {

    // if (_shape_pointcloud.lidar_prep.size() > 0)
    //   _shape_pointcloud.lidar_prep.clear();

    cout << "clusters[i].size():size:" << clusters[i].size() << endl;

    for (int j = 0; j < clusters.at(i).size(); j++)
    {

      msg_lidar_prep::LidarPoints point;
      // _shape_pointcloud.lidar_prep.resize(clusters[i].size());

      point.x = clusters.at(i).at(j).x();
      point.y = clusters.at(i).at(j).y();
      point.z = clusters.at(i).at(j).z();
      _shape_pointcloud.lidar_prep.push_back(point);
    }
    cout << "_shapepointcloud:size:" << _shape_pointcloud.lidar_prep.size() << endl;
    msg_lidar_shape::DetectedObject obj_shape;
    // call another class function
    lidar::obj::BoundingBoxModel boundingBox;
    boundingBox.estimate(_shape_pointcloud, obj_shape);

    _obj_visualization_array.objects.push_back(obj_shape);
  }
  */

  for (const auto &kv : clusters)
  {
    const auto &cluster = kv.second;

    cout << "cluster.size():size:" << cluster.size() << endl;

    for (int i = 0; i < cluster.size(); i++)
    {
      msg_lidar_prep::LidarPoints tem;
      tem.x = cluster.at(i).x();
      tem.y = cluster.at(i).y();
      tem.z = cluster.at(i).z();

      _shape_pointcloud.lidar_prep.push_back(tem);
    }

    // cout << "_shapepointcloud:size:" << _shape_pointcloud.lidar_prep.size() << endl;
    msg_lidar_shape::DetectedObject obj_shape;
    // call another class function
    lidar::obj::BoundingBoxModel boundingBox;
    boundingBox.estimate(_shape_pointcloud, obj_shape);
    _shape_pointcloud.lidar_prep.clear();

    _obj_visualization_array.objects.push_back(obj_shape);
  }

  auto endTime = std::chrono::steady_clock::now();
  auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(
      endTime - startTime);
  std::cout << "L_shape total took " << elapsedTime.count() << " milliseconds"
            << std::endl;

  // 一个封装了的发布者
  obj_visualization->ObjVisualizationMain(_obj_visualization_array);
  // 调用地面显示算法
  _obj_visualization_array.objects.clear();
  this->RvizVisual(ground_cloud, _pub_ground_cluster);
  this->RvizVisual(clusters, _pub_depth_cluster);
  _rviz_pub = 1;
  // _cluster_count = 0;
  // _pub_flag = 0;
  // locker.unlock();
}

sensor_msgs::Image Depth_cluster::DepthImage_to_ImageMsg(cv::Mat depth_image, int threshold_distance)
{
  sensor_msgs::Image depth_image_msg;
  const bool IS_BIGENDIAN = false;
  depth_image_msg.encoding = sensor_msgs::image_encodings::TYPE_8UC1;
  depth_image_msg.height = depth_image.rows;
  depth_image_msg.width = depth_image.cols;
  depth_image_msg.data.resize(depth_image_msg.height * depth_image_msg.width);
  depth_image_msg.step = depth_image_msg.width * 1;
  depth_image_msg.is_bigendian = IS_BIGENDIAN;

  for (size_t i = 0; i < depth_image_msg.height; i++)
  {
    for (size_t j = 0; j < depth_image_msg.width; j++)
    {
      if (depth_image.at<float>(i, j) >= threshold_distance)
      {
        depth_image_msg.data[i * depth_image_msg.width + j] = 255;
      }
      else
      {
        depth_image_msg.data[i * depth_image_msg.width + j] =
            static_cast<uint8_t>(depth_image.at<float>(i, j) / threshold_distance * 255);
      }
    }
  }

  return depth_image_msg;
}

sensor_msgs::Image Depth_cluster::DepthImageAndAngleImage_to_ImageMsg(cv::Mat depth_image,
                                                                      cv::Mat repaired_depth_image,
                                                                      cv::Mat angle_image,
                                                                      cv::Mat smoothed_angle_image,
                                                                      cv::Mat no_ground_depth_image)
{
  sensor_msgs::Image depth_and_angle_image_msg;
  if ((depth_image.size() != repaired_depth_image.size()) || (repaired_depth_image.size() != angle_image.size()) || (angle_image.size() != smoothed_angle_image.size()))
  {
    throw std::logic_error("unequal input cv::mat size");
  }
  const bool IS_BIGENDIAN = false;
  int num_image = 5;

  int threshold_distance = 60;

  depth_and_angle_image_msg.encoding = sensor_msgs::image_encodings::TYPE_8UC1;
  depth_and_angle_image_msg.height = depth_image.rows * num_image;
  depth_and_angle_image_msg.width = depth_image.cols;
  depth_and_angle_image_msg.step = depth_and_angle_image_msg.width * 1;
  depth_and_angle_image_msg.data.resize(depth_and_angle_image_msg.height * depth_and_angle_image_msg.step);
  depth_and_angle_image_msg.is_bigendian = IS_BIGENDIAN;

  for (size_t i = 0; i < depth_and_angle_image_msg.height / num_image; i++)
  {
    for (size_t j = 0; j < depth_and_angle_image_msg.width; j++)
    {
      if (depth_image.at<float>(i, j) > threshold_distance)
      {
        depth_image.at<float>(i, j) = threshold_distance;
      }
      depth_and_angle_image_msg.data[i * depth_and_angle_image_msg.width + j] =
          static_cast<uint8_t>(depth_image.at<float>(i, j) / threshold_distance * 255);
    }
  }
  for (size_t i = 0; i < depth_and_angle_image_msg.height / num_image; i++)
  {
    for (size_t j = 0; j < depth_and_angle_image_msg.width; j++)
    {
      if (repaired_depth_image.at<float>(i, j) > threshold_distance)
      {
        repaired_depth_image.at<float>(i, j) = threshold_distance;
      }
      depth_and_angle_image_msg.data[(i + depth_and_angle_image_msg.height / num_image) * depth_and_angle_image_msg.width + j] =
          static_cast<uint8_t>(repaired_depth_image.at<float>(i, j) / threshold_distance * 255);
    }
  }
  for (size_t i = 0; i < depth_and_angle_image_msg.height / num_image; i++)
  {
    for (size_t j = 0; j < depth_and_angle_image_msg.width; j++)
    {
      // if (smoothed_angle_image.at<float>(i, j)<0) {
      //    smoothed_angle_image.at<float>(i, j) = 0;
      // }
      // else if(smoothed_angle_image.at<float>(i, j)>M_PI/2) {
      //    smoothed_angle_image.at<float>(i, j) = M_PI/2;
      // }
      depth_and_angle_image_msg.data[(i + depth_and_angle_image_msg.height * 2 / num_image) * depth_and_angle_image_msg.width + j] =
          static_cast<uint8_t>(angle_image.at<float>(i, j) / M_PI * 2 * 255);
    }
  }
  for (size_t i = 0; i < depth_and_angle_image_msg.height / num_image; i++)
  {
    for (size_t j = 0; j < depth_and_angle_image_msg.width; j++)
    {
      if (smoothed_angle_image.at<float>(i, j) < 0)
      {
        smoothed_angle_image.at<float>(i, j) = 0;
      }
      else if (smoothed_angle_image.at<float>(i, j) > M_PI / 2)
      {
        smoothed_angle_image.at<float>(i, j) = M_PI / 2;
      }
      depth_and_angle_image_msg.data[(i + depth_and_angle_image_msg.height * 3 / num_image) * depth_and_angle_image_msg.width + j] =
          static_cast<uint8_t>(smoothed_angle_image.at<float>(i, j) / M_PI * 2 * 255);
    }
  }
  for (size_t i = 0; i < depth_and_angle_image_msg.height / num_image; i++)
  {
    for (size_t j = 0; j < depth_and_angle_image_msg.width; j++)
    {
      if (no_ground_depth_image.at<float>(i, j) > threshold_distance)
      {
        no_ground_depth_image.at<float>(i, j) = threshold_distance;
      }
      depth_and_angle_image_msg.data[(i + depth_and_angle_image_msg.height * 4 / num_image) * depth_and_angle_image_msg.width + j] =
          static_cast<uint8_t>(no_ground_depth_image.at<float>(i, j) / threshold_distance * 255);
    }
  }

  return depth_and_angle_image_msg;
}

void Depth_cluster::ground_BFS(const PixelCoord &start, uint16_t label,
                               cv::Mat &label_image, const cv::Mat &depth_image,
                               const cv::Mat &angle_image,
                               const float &ground_remove_angle)
{
  // breadth first search
  std::queue<PixelCoord> labeling_queue;
  labeling_queue.push(start);
  // while the queue is not empty continue removing front point adding its
  // neighbors back to the queue - breadth-first-search one component
  // breadth-first-search one component: 主要策略
  // 当队列不为空时， 继续取出队列的首元素，将其邻居添加到队列的末端
  size_t max_queue_size = 0;
  while (!labeling_queue.empty())
  {
    max_queue_size = std::max(labeling_queue.size(), max_queue_size);
    // copy the current coordinate
    const PixelCoord current = labeling_queue.front();
    labeling_queue
        .pop(); // pop(0)是去掉队列的第一个元素，因为在这句前已经取出这个元素给r,
                // c
    uint16_t current_label = label_image.at<uint16_t>(current.row, current.col);

    // 增加的逻辑
    // 调节参数
    // if (current_label > 0 || fabs(angle_image.at<float>(current.row,
    // current.col)) > _ground_remove_angle)
    if (current_label > 0)
    {
      // we have already labeled this point. No need to add it.
      continue; // TODO：当前点没有被打过标签的情况，往下走
    }

    label_image.at<uint16_t>(current.row, current.col) = label;
    // if (current.row == 31)
    // {
    //     label_image.at<uint16_t>(current.row, current.col) = label;
    // }
    // else
    // {
    //     if (current.row != 0)
    //     {
    //         label_image.at<uintrenderMarker16_t>(current.row - 1,
    //         current.col) = label;
    //     }
    // }

    _count_ground = _count_ground + 1;
    // check the depth
    auto current_depth = depth_image.at<float>(current.row, current.col);
    if (current_depth < 0.0001f && current.row != 31)
    {
      // TODO: 深度值太小就不麻烦添加其邻居点到队列了
      // depth of this point is wrong, so don't bother adding it to queue
      continue;
    }

    std::vector<PointCoord> point_neighbourhood =
        this->Neighbourhood(current.row, current.col);
    // cout << "label_image.cols_____________________________ : " <<
    // label_image.cols << endl;
    for (const auto &step : point_neighbourhood)
    {
      PixelCoord neighbor = PixelCoord(step.row, step.col);
      uint16_t neigh_label =
          label_image.at<uint16_t>(neighbor.row, neighbor.col);
      // cout << "neigh_label:" << neigh_label << endl;
      if (neigh_label > 0)
      {
        // we have already labeled this one
        continue;
      }

      if (fabs(angle_image.at<float>(current.row, current.col) -
               angle_image.at<float>(neighbor.row, neighbor.col)) <
          ground_remove_angle)
      {
        labeling_queue.push(neighbor);
      }
    }
  }
}

std::vector<PointCoord> Depth_cluster::Neighbourhood(int16_t coor_row,
                                                     int16_t coor_col)
{
  PointCoord tem;
  std::vector<PointCoord> neighbourhood;
  int dx[4] = {-1, 0, 1, 0}, dy[4] = {0, 1, 0, -1};
  for (int i = 0; i < 4; i++)
  {
    tem.row = coor_row + dx[i], tem.col = coor_col + dy[i];
    if (tem.row >= 0 && tem.row < 32 && tem.col >= 0 &&
        tem.col < _num_beams) // col最大为869th 从0开始计数
    {
      neighbourhood.push_back(tem);
    }
  }
  return neighbourhood;
}

void Depth_cluster::One_Label_Bfs(const PixelCoord &start, uint16_t label,
                                  cv::Mat &label_image,
                                  const cv::Mat &no_grond_depth_image,
                                  const float &angle_tollerance)
{
  // breadth first search
  std::queue<PixelCoord> labeling_queue;
  labeling_queue.push(start);
  // while the queue is not empty continue removing front point adding its
  // neighbors back to the queue - breadth-first-search one component
  // breadth-first-search one component: 主要策略
  // 当队列不为空时， 继续取出队列的首元素，将其邻居添加到队列的末端
  size_t max_queue_size = 0;
  while (!labeling_queue.empty())
  {
    max_queue_size = std::max(labeling_queue.size(), max_queue_size);
    // copy the current coordinate
    const PixelCoord current = labeling_queue.front();
    labeling_queue
        .pop(); // pop(0)是去掉队列的第一个元素，因为在这句前已经取出这个元素给r,
                // c
    uint16_t current_label = label_image.at<uint16_t>(current.row, current.col);
    if (current_label > 0)
    {
      // we have already labeled this point. No need to add it.
      continue;
    }

    // label current point
    label_image.at<uint16_t>(current.row, current.col) = label;
    // check the depth
    auto current_depth =
        no_grond_depth_image.at<float>(current.row, current.col);
    if (current_depth < 0.0001f)
    {
      // depth of this point is wrong, so don't bother adding it to queue
      continue;
    }

    std::vector<PointCoord> point_neighbourhood =
        this->Neighbourhood(current.row, current.col);

    // cout << "label_image.cols_____________________________ : " <<
    // label_image.cols << endl;

    for (const auto &step : point_neighbourhood)
    {
      PixelCoord neighbor = PixelCoord(step.row, step.col);

      uint16_t neigh_label =
          label_image.at<uint16_t>(neighbor.row, neighbor.col);
      if (neigh_label > 0)
      {
        // we have already labeled this one
        continue;
      }
      const float &current_depth =
          no_grond_depth_image.at<float>(current.row, current.col);
      const float &neighbor_depth =
          no_grond_depth_image.at<float>(neighbor.row, neighbor.col);
      float d1 = std::max(current_depth, neighbor_depth);
      float d2 = std::min(current_depth, neighbor_depth);
      float alpha;
      if (neighbor.row != current.row) // 同一列,求垂直分辨率
      {
        // alpha = fabs(vres[31-current.row] - vres[31-neighbor.row]);
        alpha = fabs(_angle_vert_image.at<float>(current.row, current.col) -
                     _angle_vert_image.at<float>(neighbor.row, neighbor.col));
        // 调节参数1
        // alpha = fabs(vres[current.row] - vres[neighbor.row]);
        // cout << "neighbor.col:" << neighbor.col << "current.col:" <<
        // current.col << endl;
      }
      // else if (neighbor.col != current.col) // 那么在同一行，求水平分辨率
      else
      {
        // alpha = hres;
        alpha = fabs(_angle_arith_image.at<float>(current.row, current.col) -
                     _angle_arith_image.at<float>(neighbor.row, neighbor.col));
      }
      float beta = (std::atan2((d2 * sin(alpha * M_PI / 180.0)),
                               (d1 - d2 * cos(alpha * M_PI / 180.0)))) *
                   180.0 / M_PI; // 聚类部分的阈值判断 beta为度数
      if (fabs(beta) > angle_tollerance)
      {
        labeling_queue.push(neighbor);
      }
    }
  }
}

// SG平滑
cv::Mat Depth_cluster::ApplySavitskyGolaySmoothing(const cv::Mat &image,
                                                   int window_size)
{
  cv::Mat kernel = GetSavitskyGolayKernel(window_size);
  cv::Mat smoothed_image; // init an empty smoothed image
  cv::filter2D(image, smoothed_image, SAME_OUTPUT_TYPE, kernel, ANCHOR_CENTER,
               0, cv::BORDER_REFLECT101);
  return smoothed_image;
}

cv::Mat Depth_cluster::GetSavitskyGolayKernel(int window_size) const
{
  if (window_size % 2 == 0)
  {
    throw std::logic_error("only odd window size allowed");
  }
  bool window_size_ok = window_size == 5 || window_size == 7 ||
                        window_size == 9 || window_size == 11;
  if (!window_size_ok)
  {
    throw std::logic_error("bad window size");
  }
  // below are no magic constants. See Savitsky-golay filter.
  cv::Mat kernel;
  switch (window_size)
  {
  case 5:
    kernel = cv::Mat::zeros(window_size, 1, CV_32F);
    kernel.at<float>(0, 0) = -3.0f;
    kernel.at<float>(0, 1) = 12.0f;
    kernel.at<float>(0, 2) = 17.0f;
    kernel.at<float>(0, 3) = 12.0f;
    kernel.at<float>(0, 4) = -3.0f;
    kernel /= 35.0f;
    return kernel;
  case 7:
    kernel = cv::Mat::zeros(window_size, 1, CV_32F);
    kernel.at<float>(0, 0) = -2.0f;
    kernel.at<float>(0, 1) = 3.0f;
    kernel.at<float>(0, 2) = 6.0f;
    kernel.at<float>(0, 3) = 7.0f;
    kernel.at<float>(0, 4) = 6.0f;
    kernel.at<float>(0, 5) = 3.0f;
    kernel.at<float>(0, 6) = -2.0f;
    kernel /= 21.0f;
    return kernel;
  case 9:
    kernel = cv::Mat::zeros(window_size, 1, CV_32F);
    kernel.at<float>(0, 0) = -21.0f;
    kernel.at<float>(0, 1) = 14.0f;
    kernel.at<float>(0, 2) = 39.0f;
    kernel.at<float>(0, 3) = 54.0f;
    kernel.at<float>(0, 4) = 59.0f;
    kernel.at<float>(0, 5) = 54.0f;
    kernel.at<float>(0, 6) = 39.0f;
    kernel.at<float>(0, 7) = 14.0f;
    kernel.at<float>(0, 8) = -21.0f;
    kernel /= 231.0f;
    return kernel;
  case 11:
    kernel = cv::Mat::zeros(window_size, 1, CV_32F);
    kernel.at<float>(0, 0) = -36.0f;
    kernel.at<float>(0, 1) = 9.0f;
    kernel.at<float>(0, 2) = 44.0f;
    kernel.at<float>(0, 3) = 69.0f;
    kernel.at<float>(0, 4) = 84.0f;
    kernel.at<float>(0, 5) = 89.0f;
    kernel.at<float>(0, 6) = 84.0f;
    kernel.at<float>(0, 7) = 69.0f;
    kernel.at<float>(0, 8) = 44.0f;
    kernel.at<float>(0, 9) = 9.0f;
    kernel.at<float>(0, 10) = -36.0f;
    kernel /= 429.0f;
    return kernel;
  }
  return kernel;
}

Mat Depth_cluster::ZeroOutGround(const cv::Mat &image,
                                 const cv::Mat &angle_image,
                                 const Radians &threshold) const // 函数重载
{
  // TODO(igor): test if its enough to remove only values starting from the
  // botom pixel. I don't like removing all values based on a threshold.
  // But that's a start, so let's stick with it for now.
  Mat res = cv::Mat::zeros(image.size(), CV_32F);
  for (int r = 0; r < image.rows; ++r)
  {
    for (int c = 0; c < image.cols; ++c)
    {
      if (angle_image.at<float>(r, c) > threshold.val())
      {
        res.at<float>(r, c) = image.at<float>(r, c);
      }
    }
  }
  return res;
}

Mat Depth_cluster::GetUniformKernel(int window_size, int type) const
{
  if (window_size % 2 == 0)
  {
    throw std::logic_error("only odd window size allowed");
  }
  Mat kernel = Mat::zeros(window_size, 1, type);
  kernel.at<float>(0, 0) = 1;
  kernel.at<float>(window_size - 1, 0) = 1;
  kernel /= 2;
  return kernel;
}

const cv::Mat &Depth_cluster::depth_image() const { return this->_depth_image; }

cv::Mat &Depth_cluster::depth_image() { return this->_depth_image; }

cv::Mat Depth_cluster::RepairDepth(const cv::Mat &no_ground_image, int step,
                                   float depth_threshold)
{
  Mat inpainted_depth = no_ground_image.clone();
  for (int c = 0; c < inpainted_depth.cols; ++c)
  {
    for (int r = 0; r < inpainted_depth.rows; ++r)
    {
      float &curr_depth = inpainted_depth.at<float>(r, c);
      if (curr_depth < 0.001f)
      {
        int counter = 0;
        float sum = 0.0f;
        for (int i = 1; i < step; ++i)
        {
          if (r - i < 0)
          {
            continue;
          }
          for (int j = 1; j < step; ++j)
          {
            if (r + j > inpainted_depth.rows - 1)
            {
              continue;
            }
            const float &prev = inpainted_depth.at<float>(r - i, c);
            const float &next = inpainted_depth.at<float>(r + j, c);
            if (prev > 0.001f && next > 0.001f &&
                fabs(prev - next) < depth_threshold)
            {
              sum += prev + next;
              counter += 2;
            }
          }
        }
        if (counter > 0)
        {
          curr_depth = sum / counter;
        }
      }
    }
  }
  return inpainted_depth;
}

visualization_msgs::Marker Depth_cluster::shape_estimate(
    const Cloud &cluster, int setid, double centroid_x, double centroid_y,
    double centroid_z, double width, double length, double height)
{
  // std::cout << "calling BoundingBoxModel::estimate" << std::endl;
  // visualization_msgs::MarkerArray marker_array;
  visualization_msgs::Marker object_marker;
  // calc centroid point for bounding_box height(z)
  // msg_lidar_prep::LidarPoints centroid;

  object_marker = CreateWireframeMarker(centroid_x, centroid_y, centroid_z,
                                        width, length, height);
  object_marker.lifetime = ros::Duration(0.5);
  object_marker.color.a = 0.5;
  object_marker.color.r = object_marker.color.g = object_marker.color.b = 1.0;
  object_marker.frame_locked = false;
  object_marker.id = setid;
  object_marker.ns = "UnKown";
  object_marker.scale.x = 0.05;
  object_marker.scale.y = 0.05;
  object_marker.scale.z = 0.05;
  object_marker.type = visualization_msgs::Marker::LINE_LIST;
  object_marker.action = visualization_msgs::Marker::MODIFY;
  object_marker.header.stamp = ros::Time::now();
  object_marker.header.frame_id = "base_link";

  return object_marker;
}

visualization_msgs::Marker Depth_cluster::CreateWireframeMarker(
    const float &center_x, const float &center_y, const float &center_z,
    const float &size_x, const float &size_y, const float &size_z)
{
  visualization_msgs::Marker box_marker;
  box_marker.pose.position.x = center_x;
  box_marker.pose.position.y = center_y;
  box_marker.pose.position.z = center_z;
  geometry_msgs::Point p1, p2, p3, p4, p5, p6, p7, p8;
  // size_y = (size_y <= 0.1f) ? 0.1f : size_y;
  // size_x = (size_x <= 0.1f) ? 0.1f : size_x;
  float half_x = (0.5) * size_x;
  float half_y = (0.5) * size_y;
  float half_z = (0.5) * size_z;

  p1.x = half_x;
  p1.y = half_y;
  p1.z = size_z;

  p2.x = half_x;
  p2.y = -half_y;
  p2.z = size_z;

  p3.x = -half_x;
  p3.y = -half_y;
  p3.z = size_z;

  p4.x = -half_x;
  p4.y = half_y;
  p4.z = size_z;

  p5 = p1;
  p5.z = -size_z;

  p6 = p2;
  p6.z = -size_z;

  p7 = p3;
  p7.z = -size_z;

  p8 = p4;
  p8.z = -size_z;

  box_marker.points.reserve(24);
  box_marker.points.push_back(p1);
  box_marker.points.push_back(p2);
  box_marker.points.push_back(p2);
  box_marker.points.push_back(p3);
  box_marker.points.push_back(p3);
  box_marker.points.push_back(p4);
  box_marker.points.push_back(p4);
  box_marker.points.push_back(p1);
  box_marker.points.push_back(p1);
  box_marker.points.push_back(p5);
  box_marker.points.push_back(p2);
  box_marker.points.push_back(p6);
  box_marker.points.push_back(p3);
  box_marker.points.push_back(p7);
  box_marker.points.push_back(p4);
  box_marker.points.push_back(p8);
  box_marker.points.push_back(p5);
  box_marker.points.push_back(p6);
  box_marker.points.push_back(p6);
  box_marker.points.push_back(p7);
  box_marker.points.push_back(p7);
  box_marker.points.push_back(p8);
  box_marker.points.push_back(p8);
  box_marker.points.push_back(p5);

  return box_marker;
}

void Depth_cluster::RvizVisual(
    std::unordered_map<uint16_t, Cloud> &visual_clusters,
    ros::Publisher puber)
{
  sensor_msgs::PointCloud msg_lidar_prep_rviz;
  msg_lidar_prep_rviz.header.stamp = ros::Time::now();
  msg_lidar_prep_rviz.header.frame_id = "innovusion";

  // we'll also add an intensity channel to the cloud
  //_msg_lidar_prep_rviz.channels.resize(2);
  //_msg_lidar_prep_rviz.channels[0].name = "intensities";
  //_msg_lidar_prep_rviz.channels[0].values.resize(_point_count);
  // 可视化这个cluster
  // cout << "visual_clusters size: " << visual_clusters.size() << endl;
  for (int i = 0; i < visual_clusters.size(); i++)
  {
    for (int j = 0; j < visual_clusters[i].size(); ++j)
    {
      geometry_msgs::Point32 tem;
      tem.x = visual_clusters.at(i).at(j).x();
      tem.y = visual_clusters.at(i).at(j).y();
      tem.z = visual_clusters.at(i).at(j).z();
      msg_lidar_prep_rviz.points.push_back(tem);
    }
  }

  //清零
  cout << "cluster point size____________:" << msg_lidar_prep_rviz.points.size()
       << endl;
  puber.publish(msg_lidar_prep_rviz);
  msg_lidar_prep_rviz.points.clear();
}

void Depth_cluster::RvizVisual(Cloud::Ptr visual_clusters,
                               ros::Publisher puber)
{
  sensor_msgs::PointCloud lidar_point_rviz;
  lidar_point_rviz.header.stamp = ros::Time::now();
  lidar_point_rviz.header.frame_id = "innovusion";
  // lidar_point_rviz.header.frame_id = "innovusion";

  for (int i = 0; i < visual_clusters->size(); i++)
  {
    geometry_msgs::Point32 tem;
    tem.x = visual_clusters->at(i).x();
    tem.y = visual_clusters->at(i).y();
    tem.z = visual_clusters->at(i).z();
    lidar_point_rviz.points.push_back(tem);
  }
  //清零
  cout << "prep rviz size____________:" << lidar_point_rviz.points.size()
       << endl;
  puber.publish(lidar_point_rviz);
  lidar_point_rviz.points.clear();
}

void Depth_cluster::RvizVisual(
    const msg_lidar_prep::msg_lidar_prep::ConstPtr &points,
    ros::Publisher puber)
{
  sensor_msgs::PointCloud lidar_point_rviz;
  lidar_point_rviz.header.stamp = ros::Time::now();
  lidar_point_rviz.header.frame_id = "innovusion";

  for (size_t index = 0; index < points->lidar_prep.size(); ++index)
  {
    const auto &point = points->lidar_prep[index]; // todo:传引用

    geometry_msgs::Point32 tem;
    tem.x = point.x;
    tem.y = point.y;
    tem.z = point.z;
    lidar_point_rviz.points.push_back(tem);
  }
  //清零
  puber.publish(lidar_point_rviz);
  lidar_point_rviz.points.clear();
}

void Depth_cluster::ViewerPcl(
    ProcessPointClouds<pcl::PointXYZI> *point_processor,
    pcl::visualization::PCLVisualizer::Ptr &viewer)
{
  // std::unordered_map<uint16_t, depth_clustering::Cloud> input_clusters = new
  // std::unordered_map<uint16_t, depth_clustering::Cloud>();
  // std::unordered_map<uint16_t, depth_clustering::Cloud> *input_clusters(new
  // std::unordered_map<uint16_t, depth_clustering::Cloud>);
  std::unordered_map<uint16_t, Cloud> input_clusters;
  // Cloud::Ptr ground_cloud;
  Cloud::Ptr ground_cloud(new Cloud); // new一个空间后再深复制
  // 访问共享内存q前先加锁
  std::unique_lock<std::mutex> locker(_mu);
  // 添加一个lamuda函数避免伪激活，当q不为空时才会被激活,
  // 这里的条件为_cluster_count!=0
  _cond.wait(locker, [this]() { return 1 == _pub_flag; });

  // input_clusters = _obj_cluster; TODO(ryon): 拷贝赋值运算符
  // 只能直接利用_obj_cluster可视化 ground_cloud = _ground_cloud; 深拷贝
  for (size_t i = 0; i < _ground_cloud->size(); i++)
  {
    ground_cloud->push_back(_ground_cloud->at(i));
  }

  // int i = 0;
  // for (const auto &kv : _obj_cluster)
  // {
  //     const auto cluster = kv.second;
  //     //Cloud::Ptr tmp_cloud(new Cloud);
  //     for (size_t j = 0; j < cluster.size(); j++)
  //     {
  //         //*(input_clusters)[i].push_back(cluster.at(j));
  //         input_clusters.at(i).push_back(cluster.at(j));
  //     }
  //     i++;
  // }
  input_clusters.swap(_obj_cluster);

  // cout << "_obj_cluster size: " << _obj_cluster.size() << endl;
  // cout << "_ground_cloud size: " << _ground_cloud->size() << endl;
  // cout << "input_clusters size: " << input_clusters.size() << endl;
  // cout << "ground_cloud size: " << ground_cloud->size() << endl;

  // 数据清空
  _obj_cluster.clear();   // 暂时不需要，因为每一次循环map都更新
  _ground_cloud->clear(); // 暂时不需要每一次循环cloud::Ptr都更新
  // _ground_cloud->points.clear();
  _cluster_count = 0;
  _pub_flag = 0;

  locker.unlock();

  cout << "_obj_cluster size:" << input_clusters.size() << endl;

  // 可视化地面以及障碍物
  CityBlock(input_clusters, ground_cloud, viewer, point_processor);

  viewer->spinOnce();
}

void Depth_cluster::CityBlock(
    const std::unordered_map<uint16_t, Cloud> &visual_clusters,
    Cloud::Ptr ground_cloud, pcl::visualization::PCLVisualizer::Ptr &viewer,
    ProcessPointClouds<pcl::PointXYZI> *point_processor)
{
  // 可视化地面
  pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_ground(
      new pcl::PointCloud<pcl::PointXYZI>);
  pcl_ground = CloudToPcl(ground_cloud);
  RenderPointCloud(viewer, pcl_ground, "plane", Color(1, 1, 0));

  int cluster_id = 0;
  std::vector<Color> colors = {Color(1, 0, 0), Color(0, 1, 0), Color(0, 0, 1)};

  // cout << "visual_clusters size_____:" << visual_clusters.size() << endl;

  for (auto &kv : visual_clusters)
  {
    // 有很多个不同标签的cluster
    const auto &cluster = kv.second;
    pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_cluster(
        new pcl::PointCloud<pcl::PointXYZI>);
    // cout << "cluster size_____:" << cluster.size() << endl;
    pcl_cluster = CloudToPcl(cluster);
    // render cluster point cloudC
    RenderPointCloud(viewer, pcl_cluster,
                     "obstacle_cloud " + std::to_string(cluster_id),
                     colors[cluster_id]);

    // render box
    Box box = point_processor->BoundingBox(pcl_cluster);
    renderBox(viewer, box, cluster_id, Color(1, 0, 0), 1);
    // pcl_cluster->clear();
    // pcl_cluster->points.clear();
    cluster_id++;
  }
}

pcl::PointCloud<pcl::PointXYZI>::Ptr Depth_cluster::CloudToPcl(
    const Cloud &PointCloud)
{
  pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_cluster(
      new pcl::PointCloud<pcl::PointXYZI>);
  pcl::PointXYZI point_pcl;
  // cout << "cluster PointCloud.size():_________" << PointCloud.size() << endl;
  for (size_t i = 0; i < PointCloud.size(); i++)
  {
    // point_pcl.x = PointCloud->points.at(i).x();
    // point_pcl.y = PointCloud->points.at(i).y();
    // point_pcl.z = PointCloud->points.at(i).z();
    // point_pcl.intensity = PointCloud->points.at(i).x();

    point_pcl.x = PointCloud.at(i).x();
    point_pcl.y = PointCloud.at(i).y();
    point_pcl.z = PointCloud.at(i).z();
    point_pcl.intensity = PointCloud.at(i).intensity();
    // pcl_cluster->push_back(point_pcl);
    pcl_cluster->points.push_back(point_pcl);
  }
  return pcl_cluster;
}

void Depth_cluster::RenderPointCloud(
    pcl::visualization::PCLVisualizer::Ptr &viewer,
    const pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud, std::string name,
    Color color)
{
  if (color.r == -1)
  {
    // Select color based off of cloud intensity
    pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI>
        intensity_distribution(cloud, "intensity");
    viewer->addPointCloud<pcl::PointXYZI>(cloud, intensity_distribution, name);
  }
  else
  {
    // Select color based off input value
    viewer->addPointCloud<pcl::PointXYZI>(cloud, name);
    viewer->setPointCloudRenderingProperties(
        pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b,
        name);
  }

  viewer->setPointCloudRenderingProperties(
      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, name);
}

pcl::PointCloud<pcl::PointXYZI>::Ptr Depth_cluster::CloudToPcl(
    const Cloud::Ptr PointCloud)
{
  pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_cluster(
      new pcl::PointCloud<pcl::PointXYZI>);
  pcl::PointXYZI point_pcl;
  // cout << "PointCloud->size():_________" << PointCloud->size() << endl;
  for (size_t i = 0; i < PointCloud->size(); i++)
  {
    // point_pcl.x = PointCloud->points.at(i).x();
    // point_pcl.y = PointCloud->points.at(i).y();
    // point_pcl.z = PointCloud->points.at(i).z();
    // point_pcl.intensity = PointCloud->points.at(i).x();

    point_pcl.x = PointCloud->at(i).x();
    // cout << "point_pcl.x:_____________________ " << point_pcl.x << endl;
    point_pcl.y = PointCloud->at(i).y();
    point_pcl.z = PointCloud->at(i).z();
    point_pcl.intensity = PointCloud->at(i).intensity();
    // pcl_cluster->push_back(point_pcl);
    pcl_cluster->points.push_back(point_pcl);
  }
  return pcl_cluster;
}